#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Copyright (C) 2023 reinovo, Inc. All Rights Reserved 
#
# @Time    : 2024/1/10 上午10:16
# @Author  : hmm
# @Email   : liuyuhang0531@foxmail.com
# @File    : test.py
# @Software: PyCharm

from arm_control import uarm_control
import cv2
import numpy as np
import time

mtx = np.load('../data/camera_param.npz')['mtx']
with np.load('../data/eyehand_Matrix.npz') as X:
    R, t = [X[i] for i in ('R', 't')]

arm = uarm_control.Arm_controller(mtx,R,t)
arm.arm_init()

print("----------------------")
wh = arm.make_warehouse([180,80,10],[2,2,2],[30,30,10])
arm.move([200,0,150])
cv2.destroyAllWindows()
cap = cv2.VideoCapture("/dev/video0")
_, frame = cap.read()
cv2.imshow("image",frame)
cv2.waitKey()
cap.release()
cv2.destroyAllWindows()

i=0
j = 0
while i < 12:
    point = arm.tag_point(frame,i,20)
    if len(point) > 0:
        # print(wh[i])
        pos = point[0]
        print(pos)
        start_time = time.time()
        arm.gate_move([pos[0],pos[1],pos[2]], wh[j], 50, offset_angle=-pos[3][2])
        print(f"运行时间为{time.time()-start_time}")
        arm.move([200, 0, 150])
        j += 1
    i += 1





